import rosbag
from colorama import Fore, Style
import os
import sys
import warnings
import def_function
from datetime import datetime

# 获取当前脚本的完整路径
script_path = os.path.abspath(sys.argv[0])
# 从完整路径中获取目录
script_dir = os.path.dirname(script_path)
# 从完整路径中分离出文件名
script_name = os.path.basename(script_path)
# 使用 splitext() 函数分离文件名和扩展名
script_name_without_extension, extension = os.path.splitext(script_name)

print(Fore.YELLOW+f"python script name:", sys.argv[0])
if len(sys.argv)!=2:
    print(Fore.RED+f"you need python params!")
    sys.exit()

current_rosbag= script_dir + "/" + sys.argv[1]
if os.path.isabs(sys.argv[1]):
    current_rosbag = sys.argv[1]

#print(Fore.WHITE+f"current_rosbag:", current_rosbag)
rosbag_data = rosbag.Bag(current_rosbag)
# 获取类型和主题信息
# rosbag_info = rosbag_data.get_type_and_topic_info()
# info.topics 是一个字典，它的键是 topic 的名字
# all_topics = list(rosbag_info.topics.keys())
topics = ['/vehicle/chassis_report', '/vehicle/wheel_report']

error_msg = ""
error_msg_arry = []
record_time = 0.0
start_time = 0.0
end_time = 0.0
# 创建一个 ValueJudge 对象
judge = def_function.judge_value.ValueJudge()

standstill = True
wheel_position_stanstill = True
wheel_position_front_left = None
wheel_position_front_right = None
wheel_position_rear_left = None
wheel_position_rear_right = None
wheel_position_front_left_delta = 0
wheel_position_front_right_delta = 0
wheel_position_rear_left_delta = 0
wheel_position_rear_right_delta = 0

for msg_topic, msg, t in rosbag_data.read_messages(topics=topics):
    if msg_topic == '/vehicle/chassis_report':
        standstill = msg.brake_info_report.brake_info_report_data.stationary
    if msg_topic == '/vehicle/wheel_report':
        if wheel_position_front_left is None:
            wheel_position_front_left = msg.wheel_position_report.wheel_position_report_data.front_left
        else:
            wheel_position_front_left_delta = msg.wheel_position_report.wheel_position_report_data.front_left - wheel_position_front_left
            wheel_position_front_left = msg.wheel_position_report.wheel_position_report_data.front_left
        
        if wheel_position_front_right is None:
            wheel_position_front_right = msg.wheel_position_report.wheel_position_report_data.front_right
        else:
            wheel_position_front_right_delta = msg.wheel_position_report.wheel_position_report_data.front_right - wheel_position_front_right
            wheel_position_front_right = msg.wheel_position_report.wheel_position_report_data.front_right

        if wheel_position_rear_left is None:
            wheel_position_rear_left = msg.wheel_position_report.wheel_position_report_data.rear_left
        else:
            wheel_position_rear_left_delta = msg.wheel_position_report.wheel_position_report_data.rear_left - wheel_position_rear_left
            wheel_position_rear_left = msg.wheel_position_report.wheel_position_report_data.rear_left

        if wheel_position_rear_right is None:
            wheel_position_rear_right = msg.wheel_position_report.wheel_position_report_data.rear_right
        else:
            wheel_position_rear_right_delta = msg.wheel_position_report.wheel_position_report_data.rear_right - wheel_position_rear_right
            wheel_position_rear_right = msg.wheel_position_report.wheel_position_report_data.rear_right
    
    if 0 < wheel_position_front_left_delta or 0 < wheel_position_front_right_delta  or 0 < wheel_position_rear_left_delta or 0 < wheel_position_rear_right_delta:
        wheel_position_stanstill = False
    else:
        wheel_position_stanstill = True


    result = judge.timeout(t.to_sec(), standstill, wheel_position_stanstill, 0.3, "!="); 
    # 逻辑是 300ms，判断400ms
    if result == True:
        if start_time == 0.0:
            start_time = t.to_sec()
            error_msg = "[" + str(record_time) + "-" + str(t.to_sec()) + "]standstill信号未与轮速脉冲静止状态保持一致, 车辆静止[" + str(standstill) + "]-脉冲静止[" + str(wheel_position_stanstill) + "][四轮轮速脉冲计数, 二帧数据未发生变化,脉冲静止。持续300ms车辆静止&脉冲静止不同步]"
    else:
        end_time = t.to_sec()
        if start_time != 0.0:
            error_msg = "[" + str(start_time) + "-" + str(end_time) + "]" + error_msg
            error_msg_arry.append(error_msg)
            start_time = 0.0

if start_time != 0.0:
    end_time = t.to_sec()
    error_msg_arry.append("[" + str(start_time) + "-" + str(end_time) + "]" + error_msg)

# 打印数组中的每个元素, 每个元素一行
for item in error_msg_arry:
    print(Fore.RED+f"", item)

rosbag_data.close()
